#include "simd.h"

/**
 * 2 dimension gaussian filter
 * @param src
 * @param dst
 * @param window
 */
void avxGaussianFilter(Mat &src, Mat &dst, Mat window) {
    int hh = (window.rows - 1) / 2;
    int hw = (window.cols - 1) / 2;
    dst = Mat::zeros(src.size(), src.type());
    //边界填充
    Mat newSrc;
    copyMakeBorder(src, newSrc, hh, hh, hw, hw, BORDER_REPLICATE);//边界复制
    //高斯滤波
    double sum;
    cout << window << endl;
    auto *windowVector = new float[window.rows * window.cols];
    int k = 0;
    for (int i = 0; i < window.rows; i++) {
        for (int j = 0; j < window.cols; j++) {
            windowVector[k++] = window.at<float>(i, j);
        }
    }
    for (int i = hh; i < newSrc.rows - hh; ++i) {
        for (int j = hw; j < newSrc.cols - hw; ++j) {
            sum = 0;
            int r = 0;
            float temp1[8] = {0};
            float temp2[8] = {0};
            __m256 t1, t2, _sum, _mul;
            auto *srcVector = new float[window.rows * window.cols];
            int k = 0;
            for (int sr = -hh; sr <= hh; sr++) {
                for (int sc = -hw; sc <= hw; sc++) {
                    srcVector[k++] = newSrc.at<float>(i + sr, j + sc);
                }
            }
            _sum = _mm256_loadu_ps(temp1);
            for (; r + 8 < window.rows * window.cols; r += 8) {
                t1 = _mm256_loadu_ps(&windowVector[r]);
                t2 = _mm256_loadu_ps(&srcVector[r]);
                _mul = _mm256_mul_ps(t1, t2);
                _sum = _mm256_add_ps(_sum, _mul);
                _mm256_storeu_ps(temp2, _sum);
                for (float t : temp2) {
                    sum += t;
                }
            }
            for (; r < window.rows * window.cols; r++) {
                sum = sum + srcVector[r] * windowVector[r];
            }
            free(srcVector);

            if (sum < 0)
                sum = 0;
            else if (sum > 255)
                sum = 255;

            dst.at<uchar>(i - hh, j - hw) = static_cast<uchar>(sum);

        }
    }

}
